#include <Arduino.h>
#include "FastAccelStepper.h"
#include <ESP32Servo.h>


#include <EEPROM.h>
#include <WiFi.h>
#include <ESPmDNS.h>

#include <AsyncTCP.h>
#include <ESPAsyncWebServer.h>
#include <AsyncElegantOTA.h>

#define MQTT_MAX_PACKET_SIZE 512              //override MQTT_MAX_PACKET_SIZE(256) in pubsubclient.h, to fit the mqtt payload needs
#include <PubSubClient.h>                     //这条include 必须在 MQTT_MAX_PACKET_SIZE 定义之后，否则会有警告
#include <ArduinoJson.h>

// As in StepperDemo for Motor 1 on ESP32
// #define enablePinStepper 4
#define x_pul  15
#define x_dir  2
#define y_pul  4
#define y_dir  16
#define z_pul  17
#define z_dir  5
// 原点监测传感器
#define MIN_X            34
#define MIN_Y            35
#define MIN_Z            32

#define MAX_X            33
#define MAX_Y            25
#define MAX_Z            26

#define MIN_STEP_uS          5          //最小补偿脉冲宽度，步进电机驱动器最高频率 200KHZ,脉宽最小5uS
#define X_ACCELERATE         2000
#define Y_ACCELERATE         2000
#define Z_ACCELERATE         2000

#define PICK_SERVO_PIN       21
#define SERVO_minUs        	 500
#define SERVO_maxUs        	 2500

//----------------------------------------

#define TX_PIN 21         // Rx pin which the S8 Tx pin is attached to (change if it is needed)
#define RX_PIN 22         // Tx pin which the S8 Rx pin is attached to (change if it is needed)
//---------EEPROM 地址分区映射----------
#define EEPROM_SIZE   512               //存储和恢复数据点，防止重启后执行器状态变更
#define WIFI_CON_MAX_TRY  50            //WIFI最大尝试连接次数
#define MQTT_CON_MAX_TRY  5             //MQTT最大尝试连接次数
#define FW_VERSION   1.2                    //程序版本号，用于升级标识
#define MQTT_UPLOAD_CYCLE 30                //mqtt 主动上报周期:秒

//----------------------------------------
typedef struct dp_tag
{
  int32_t x;              //
  int32_t y;              //
  int32_t z;              //

  int32_t delta_x;        //
  int32_t delta_y;        //
  int32_t delta_z;        //

  char cmd[10];           //控制指令

  int32_t pick_servo_ang; //

  uint16_t rssi;          //RSSI
} dev_data_t;


//------------------------------------------------------------------
FastAccelStepperEngine engine = FastAccelStepperEngine();
FastAccelStepper *x_stepper = NULL;
FastAccelStepper *y_stepper = NULL;
FastAccelStepper *z_stepper = NULL;

Servo pickServo; 				  // create servo object to control a servo

dev_data_t x_devdatapoint; //数据点

// Replace with your network credentials
const char* ssid     = "xjaujsj";
const char* password = "1223334444";


// String device_rec_topic = JXB_CtrlerNO1_ID + "/+" ;
const char* userName = "xjau";                            //这里用户名和密码可以是企业用户，也可以是web用户
const char* passWord = "xjau1952";                        //尽量使用企业用户，然后用二维码绑定到web用户
// Add your MQTT Broker IP address
const char* mqtt_server = "192.168.1.232";
const uint16_t mqtt_port = 1883;

String deviceID = "ctrlrNO1";                               //xyz+剪刀控制器ID

String xyzTable_Ctrler_ID = "slidTable_" + deviceID;
String scissor_Ctrler_ID  = "scissor_" + deviceID;

WiFiClient espClient;
PubSubClient client(espClient);
String uploadJson;                                       // mqtt publish payload

AsyncWebServer server(80);

//------------IPC线程通信对象-----------
typedef struct msg_q_tag {
  char cmd[10];          //设备控制命令
  int32_t v1;            //控制命令参数1
  int32_t v2;            //控制命令参数2
  int32_t v3;            //控制命令参数3
} ctrl_msg_queue_t;     //接收到的MOSQUITTO命令解析后发往执行线程的MSG Q

xQueueHandle g_mqtt_ctrl_msgQ;      //tlink下发控制命令消息队列句柄

//------TASK HANDLER-------
TaskHandle_t mqttTaskHandle = NULL;                  // mqtt keyservice tsk handle
TaskHandle_t mqttExcutorTaskHandle = NULL;           // mqtt cmd executor tsk handle

uint8_t dataPoint_EPPROM_shadowReg[EEPROM_SIZE];     //创建要放入EEPROM的数组，用于memecp 到 x_devdatapoint数据点


//-------------------------------------------------------------------
//++++++++++++++++++++++++++++ static function define ++++++++++++++++++++++++++++++++++
//----函数声明-----
void system_software_reset(void);
void mqtt_upload_json(void);


/********************************************************
* Function name: setup_wifi
* Description  : poll for modbus holding reg data
* Parameter    : void

* return        void
**********************************************************/
static void setup_wifi(void) {
  uint8_t wifi_connect_counter = WIFI_CON_MAX_TRY;

  WiFi.disconnect();               //防止已经连接了WIFI,显著提升复位后WIFI 连接速度
  delay(10);
  // start by connecting to a WiFi network
  Serial.printf("\r\nConnecting to %s\r\n", ssid);
  WiFi.mode(WIFI_STA);
  WiFi.begin(ssid, password);      //connect to wifi AP

  while (WiFi.status() != WL_CONNECTED) {
    delay(1000);
    Serial.print(".");
    if(--wifi_connect_counter < 1){
      system_software_reset();        //save datapoint and reset
    }
  }

  Serial.println("WiFi connected!");
  Serial.println("IP address: ");
  Serial.println(WiFi.localIP());
}


/********************************************************
* Function name: OTA_webserver_init
* Description  : AsyncElegantOTA 升级初始化，创建两个路由：
                 /根目录展示json
                 /update 升级fermware
* Parameter    : void

* return        void
**********************************************************/
void OTA_webserver_init(void){
  server.on("/", HTTP_GET, [](AsyncWebServerRequest *request) {
    request->send(200, "text/plain", uploadJson.c_str());
  });

  AsyncElegantOTA.begin(&server);    // Start ElegantOTA
  server.begin();
  Serial.println("HTTP server started, OTA server started");
}


/********************************************************
* Function name: callback
* Description  : MQTT 接收消息回调函数
* Parameter    :
* @topic:      传感器节点收到的 mqtt topic
* @message:    传感器节点收到的 mqtt message payload
* @length:     payload长度
* return        void
**********************************************************/
void mqtt_rec_callback(char* topic, byte* message, unsigned int length) {

  ctrl_msg_queue_t mqtt_ctrl_msg;                       //接收到的MOSQUITTO控制消息，包含key:读写标识 和 value:控制量
  String mqttTopic(topic);
  String messageJson(message,length) ;                   //mqtt message from server,字符数组构造字符串对象
  Serial.printf("\r\nmqtt topic: %s\r\n",mqttTopic.c_str());     //print message
  Serial.printf("\r\nMessage payload: %s\r\n",messageJson.c_str());     //print message

  // String input;

  StaticJsonDocument<256> doc;

  DeserializationError error = deserializeJson(doc, messageJson);

  if (error) {
    Serial.print("deserializeJson() failed: ");
    Serial.println(error.c_str());
    return;
  }

  long x = doc["x"]; // -1
  long y = doc["y"]; // -1
  long z = doc["z"]; // -1
  // long rssi = doc["rssi"]; // 65535
  const char* cmd = doc["cmd"]; // "AMOV"
  // int pwm1 = doc["pwm1"]; // 1234
  // int pwm2 = doc["pwm2"]; // 4567
  // int delta_x = doc["delta_x"]; // 1
  // int delta_y = doc["delta_y"]; // 1
  // int delta_z = doc["delta_z"]; // 1

  memcpy(mqtt_ctrl_msg.cmd, cmd, sizeof("CMDT"));     //命令长度固定为4byte
  mqtt_ctrl_msg.v1 = x;
  mqtt_ctrl_msg.v2 = y;
  mqtt_ctrl_msg.v3 = z;

	if (xQueueSend(g_mqtt_ctrl_msgQ, &mqtt_ctrl_msg, 2000) == pdPASS)
  {
    Serial.println("mqtt cmd send to execute thread");
  }

  // mqtt_upload_json();     //通过数据下发功能，可触发callback的 ACK，达到按照用户需求实时刷新目的

}
/********************************************************
* Function name: mqtt_reconnect
* Description  : 重新连接MOSQUITTO 服务器，超过最大连接数后重启ESP32
* Parameter    : void

* return        void
**********************************************************/
void mqtt_reconnect(void) {
  uint8_t recon_counter = MQTT_CON_MAX_TRY;   //最大重连5次

  // Loop until we're reconnected
  while (!client.connected()) {
    if(--recon_counter < 1){
      Serial.println("reconnect failed after 5 times of trying, reset ESP32...");
      system_software_reset();        //save datapoint and reset
    }

    Serial.print("Attempting MQTT connection...");
    // Attempt to connect
    if (client.connect(deviceID.c_str(),userName,passWord)) {
      Serial.println("device connected");
      // Subscribe
      String device_rec_topic = "/" + xyzTable_Ctrler_ID + "/#" ;   //订阅设备所有topic /XYZ滑台TOPIC
      client.subscribe(device_rec_topic.c_str());                  //订阅设备所有topic

      device_rec_topic.clear();
      device_rec_topic = "/" + scissor_Ctrler_ID + "/#" ;          //订阅设备所有topic /舵机夹子TOPIC
      client.subscribe(device_rec_topic.c_str());                  //订阅设备所有topic
    } else {
      Serial.print("failed, rc=");
      Serial.print(client.state());
      Serial.println(" try again in 5 seconds");
      // Wait 5 seconds before retrying
      delay(5000);
    }
  }
}


/********************************************************
* Function name: mqtt_connect
* Description  : connect to mqtt broker
* Parameter    : void

* return        void
**********************************************************/
static void mqtt_connect(void) {
  client.setServer(mqtt_server, mqtt_port);
  client.setCallback(mqtt_rec_callback);
  mqtt_reconnect();                     //连接tlink server
}

/********************************************************
* Function name: mqtt_upload_json
* Description  : 获取RSSI，通过string的拼接，构造mqtt json 数据
* Parameter    : void

* return        void
**********************************************************/
static void datapoint_to_json(void){
    //-------通过修改topic的末尾数据，可实现连续多个设备的数据转发上报功能，是数据网关的关键功能！

    //---------UPDATE RELAY STATE----------
    uploadJson.clear();
    uploadJson =  "{\"x\":";
    uploadJson += x_devdatapoint.x;
    uploadJson +=  ",\"y\":";
    uploadJson += x_devdatapoint.y;
    uploadJson +=  ",\"z\":";
    uploadJson += x_devdatapoint.z;
    uploadJson +=  ",\"rssi\":";
    uploadJson += x_devdatapoint.rssi;
    // uploadJson += "{\"flag\":\"a_t\",\"value\":";

    // uploadJson += x_devdatapoint.rssi;
    // // uploadJson += "},{\"flag\":\"FW\",\"value\":";
    // // uploadJson += FW_VERSION;
    uploadJson += "}";

    Serial.println(uploadJson.c_str());

}


/********************************************************
* Function name: mqtt_upload_json
* Description  : 获取RSSI，通过string的拼接，构造mqtt json 数据
* Parameter    : void

* return        void
**********************************************************/
void mqtt_upload_json(void){
    static uint8_t mqttUploadCnt = 10;
    datapoint_to_json();  // datapoint 对象，拼接成json，结果存在uploadJson全局字符串中

    // bool pubRes = client.publish(JXB_CtrlerNO1_ID.c_str(),uploadJson.c_str());      //发布json数据
    bool pubRes = client.publish(uploadJson.c_str(),uploadJson.c_str());      //发布json数据
    while(mqttUploadCnt > 1)
    {
      mqttUploadCnt --;
      // system_software_reset();
    }
      if(pubRes != false){
         mqttUploadCnt = 10;
      }
}



/********************************************************
* Function name: tsk_mqt_mqtt_keepalive
* Description  : 连接并保活MOSQUITTO MQTT服务器连接
* Parameter    : pvParam 线程创建传入参数


* return         void
**********************************************************/
void tsk_mqtt_service(void *pvParam)
{
  mqtt_connect();                                        //连接MOSQUITTO

  delay(3000);                                                  //
  mqtt_upload_json();                                           //upload json mqtt data 第一次设备上线上报数据
  while (1)
  {
   mqtt_reconnect();                     //连接tlink server
   client.loop();                              //MQTT消息处理函数
   delay(100);
  }
}
/********************************************************
* Function name: steper_motor_init
* Description  : 初始化fastAccelSteper步进电机控制对象
* Parameter    : void

* return         void
**********************************************************/
void steper_motor_init(void)
{
  //-----行程开关IO初始化----------
  pinMode(MIN_X,INPUT_PULLUP);        //X轴最小行程限位，同时也是归零点
  pinMode(MIN_Y,INPUT_PULLUP);        //Y轴最小行程限位，同时也是归零点
  pinMode(MIN_Z,INPUT_PULLUP);        //Z轴最小行程限位，同时也是归零点
  pinMode(MAX_X,INPUT_PULLUP);        //X轴最大行程限位
  pinMode(MAX_Y,INPUT_PULLUP);        //Y轴最大行程限位
  pinMode(MAX_Z,INPUT_PULLUP);        //Z轴最大行程限位

  pinMode(x_dir,OUTPUT);              //配置方向控制引脚为输出引脚
  pinMode(y_dir,OUTPUT);              //配置方向控制引脚为输出引脚
  pinMode(z_dir,OUTPUT);              //配置方向控制引脚为输出引脚

  pinMode(x_pul,OUTPUT);              //配置脉冲控制引脚为输出引脚
  pinMode(y_pul,OUTPUT);              //配置脉冲控制引脚为输出引脚
  pinMode(z_pul,OUTPUT);              //配置脉冲控制引脚为输出引脚


  //-----------------------------
  engine.init();
  x_stepper = engine.stepperConnectToPin(x_pul);
  y_stepper = engine.stepperConnectToPin(y_pul);
  z_stepper = engine.stepperConnectToPin(z_pul);
  if (x_stepper) {
    x_stepper->setDirectionPin(x_dir);
    x_stepper->setSpeedInUs(MIN_STEP_uS);  // the parameter is us/step !!!
    x_stepper->setAcceleration(X_ACCELERATE);
  }
  if (y_stepper) {
    y_stepper->setDirectionPin(y_dir);
    y_stepper->setSpeedInUs(MIN_STEP_uS);  // the parameter is us/step !!!
    y_stepper->setAcceleration(Y_ACCELERATE);
  }
  if (z_stepper) {
    z_stepper->setDirectionPin(z_dir);
    z_stepper->setSpeedInUs(MIN_STEP_uS);  // the parameter is us/step !!!
    z_stepper->setAcceleration(Z_ACCELERATE);
  }
}



/********************************************************
* Function name: tsk_executer_ctrl
* Description  : mqtt 命令接收解析处理 线程函数
* Parameter    : pvParam 线程创建传入参数

                 命令说明：rssi=-1 进入OTA远程升级
                          rssi=0 远程重启
                          rssi为其他值，招测命令

* return         void
**********************************************************/

void tsk_executer_ctrl(void *pvParam)
{
  ctrl_msg_queue_t mqtt_ctrl_msg_rec;         //接收到的MOSQUITTO控制消息，包含key:读写标识 和 value:控制量

  steper_motor_init();                        //fastaccelstepper步进电机驱动库初始化
  while (1)
  {
    if (xQueueReceive(g_mqtt_ctrl_msgQ, &mqtt_ctrl_msg_rec, MQTT_UPLOAD_CYCLE*1000) == pdTRUE){
      Serial.println("MQTT--->ESP32: ctrl massage received!!!");
		  if( String(mqtt_ctrl_msg_rec.cmd) == "OTAUPDATE"){           // OTA 升级
          Serial.println("ESP32 ENTER OTA MODE >>>>>>>>>>>");
          vTaskDelete(mqttTaskHandle);                        //删除mqtt任务，解决OTA与mqtt任务冲突问题
          if(MDNS.begin(deviceID.c_str()))                    //设置DNS域名  http://$JXB_CtrlerNO1_ID.local, 通过MOSQUITTO sn,区分域名
            {
              Serial.println("MDNS started");
              Serial.println(deviceID +".local");
            }
          Serial.printf("OTA URL: %s ",deviceID+"/update"); //OTA升级URL

          datapoint_to_json();               // 生成json字符串，供webserver显示，输出uploadJson全局变量
          OTA_webserver_init();              // OTA webserver 任务初始化，http://192.168.119.233/update访问OTA更新页面
          delay(2*60*1000);                 // OTA升级后，重启，一般不会运行到此处，OTA更新完毕自动重启
          Serial.println("OTA TIMEOUT,ESP32 IS RESETING...");
          ESP.restart();                    // 重启ESP32
      }

      if( String(mqtt_ctrl_msg_rec.cmd) == "AMOV"){               // 按照坐标运动
        z_stepper->stopMove();
      }
      if( String(mqtt_ctrl_msg_rec.cmd) == "ZERO"){               // 按照坐标运动
        Serial.println("RESET X TO ORIGIN POINT");
        x_stepper->move(100*1000);
        for(uint32_t i=0; i<100*1000; i++){
            if(HIGH == digitalRead(MIN_X)){
              Serial.println("x origin reached");
              x_stepper->forceStop();
              break;
            }
            delay(10);
            Serial.print("x");
        }
        Serial.println("RESET Y TO ORIGIN POINT");
        y_stepper->move(100*1000);
        for(uint32_t i=0; i<100*1000; i++){
            if(HIGH == digitalRead(MIN_Y)){
              Serial.println("y origin reached");
              x_stepper->forceStop();
              break;
            }
            delay(10);
            Serial.print("y");
        }
        Serial.println("RESET Z TO ORIGIN POINT");
        z_stepper->move(100*1000);
        for(uint32_t i=0; i<100*1000; i++){
            if(HIGH == digitalRead(MIN_Z)){
              Serial.println("z origin reached");
              x_stepper->forceStop();
              break;
            }
            delay(10);
            Serial.print("z");
        }
        x_stepper->stopMove();
        y_stepper->stopMove();
        z_stepper->stopMove();
      }
      if( String(mqtt_ctrl_msg_rec.cmd) == "RMOV"){               // 相对上一个位置运动
        Serial.println("relative move ctrl");
        x_stepper->move(mqtt_ctrl_msg_rec.v1*1000);
        y_stepper->move(mqtt_ctrl_msg_rec.v2*1000);
        z_stepper->move(mqtt_ctrl_msg_rec.v3*1000);
      }


    }//endofif (xQueueReceive(g_mqtt_ctrl_msgQ,
     //-----------消息队列超时控制自主上报周期，消息队列收到立即上报-------
    // mqtt_upload_json();         //upload json mqtt data
  } //end of while(1)
}





/********************************************************
* Function name: NVS_load_dataPoint
* Description  : 从non-volatile storage（flash）中载入数据点，恢复复位前状态
* Parameter    :

* return         void
**********************************************************/

static void NVS_load_dataPoint(void){
   EEPROM.begin(EEPROM_SIZE);            // initialize EEPROM with predefined size

   for(uint8_t i=0;i<sizeof(dev_data_t);i++){
    dataPoint_EPPROM_shadowReg[i] = EEPROM.read(i);
  }
  //--------RESTOR data point----------
  memcpy((void*)(&x_devdatapoint),dataPoint_EPPROM_shadowReg,sizeof(dev_data_t)); //数据恢复到g_dev_data中
  Serial.println("EEPROM data restore finished");

  //--------RESTOR EXCUTOR STATE----------

}

/********************************************************
* Function name: NVS_save_dataPoint
* Description  : 将设备数据点存入EEPROM
* Parameter    :

* return         void
**********************************************************/

static void NVS_save_dataPoint(void)
{
  memcpy(dataPoint_EPPROM_shadowReg,(void*)(&x_devdatapoint),sizeof(dev_data_t)); //memcpy到缓存
  for(uint8_t i=0;i<sizeof(dev_data_t);i++){
    EEPROM.write(i, dataPoint_EPPROM_shadowReg[i]);
  }
  delay(10);
  EEPROM.commit();
  Serial.println("EEPROM data write compplete");

}

/********************************************************
* Function name: system_software_reset
* Description  : 将设备数据点存入EEPROM后，重启ESP32，解决WIFI 无法连接、MQTT 连接断掉等问题
* Parameter    :

* return         void
**********************************************************/
void system_software_reset(void){
  NVS_save_dataPoint();         //save datapoint to EEPROM
  delay(1000);
  ESP.restart();                //after wifi_connect_counter times of trying, restart ESP32
}
//-------------------------------------------------------------------
void setup() {
  Serial.begin(115200);

  	// Allow allocation of all timers
	ESP32PWM::allocateTimer(0);
	// ESP32PWM::allocateTimer(1);
	// ESP32PWM::allocateTimer(2);
	// ESP32PWM::allocateTimer(3);

	pickServo.setPeriodHertz(50); // Standard 50hz servo
	pickServo.attach(PICK_SERVO_PIN, SERVO_minUs, SERVO_maxUs);

	Serial.println("timer init ok ");

  NVS_load_dataPoint();                 //EEPROM load函数先调用，因此在其第一行调用EEPROM.begin(size)避免遗漏！！
  setup_wifi();                          // 连接WIFI AP
  //----------ipc 对象创建--------
  g_mqtt_ctrl_msgQ = xQueueCreate(5, sizeof (ctrl_msg_queue_t));     //创建控制消息队列， mqtt callback发送至executer执行器线程

  xTaskCreateUniversal(tsk_mqtt_service, "MQTTServiceTsk",  CONFIG_ARDUINO_LOOP_STACK_SIZE, NULL, 11, &mqttTaskHandle, CONFIG_ARDUINO_RUNNING_CORE);                         // modbus任务，注意堆栈设置为8K
  xTaskCreateUniversal(tsk_executer_ctrl, "MQTTMSGCmdEXE",  CONFIG_ARDUINO_LOOP_STACK_SIZE, NULL, 12, &mqttExcutorTaskHandle, CONFIG_ARDUINO_RUNNING_CORE);                         // modbus任务，注意堆栈设置为8K


}
//-------------------------------------------------------------------
void loop() {

  // pickServo.writeMicroseconds(1000);
  // delay(3*1000);
  // pickServo.writeMicroseconds(1500);
  delay(3*1000);


}
